#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <pcl/common/common.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <sophus/se2.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

class ICP_P2P_GN
{
public:
    ICP_P2P_GN()
    {
        laser_sub_ = nh_.subscribe<sensor_msgs::LaserScan>("/scan", 1, std::bind(&ICP_P2P_GN::LaserCallBack, this, std::placeholders::_1));
    }

private:
    void LaserCallBack(const sensor_msgs::LaserScan::ConstPtr &msg)
    {
        if (first_scan_)
        {
            pcl::PointCloud<pcl::PointXY> laser_cloud;
            for (size_t i = 0; i < msg->ranges.size(); i++)
            {
                if (msg->ranges[i] < 0.1 || msg->ranges[i] > 18)
                    continue;
                double real_angle = msg->angle_min + i * msg->angle_increment;
                double x = msg->ranges[i] * cos(real_angle);
                double y = msg->ranges[i] * sin(real_angle);
                laser_cloud.points.emplace_back(pcl::PointXY{float(x), float(y)});
                first_scan_ = false;
            }
            kdtree_.setInputCloud(laser_cloud.makeShared());
            cloud_ = laser_cloud;
        }
        cur_scan_ = *msg;
        // 开始匹配
    }

    void aligin(const sensor_msgs::LaserScan::ConstPtr &msg)
    {
        // 转换到上一帧的坐标系下
        Sophus::SE2d T;
        const int iter_count = 10;
        const int min_match_num = 20;
        double last_cost = 0, cost = 0;
        const double max_dis = 0.01;
        
        for (size_t iter = 0; iter < iter_count; ++iter)
        {   
            //
            int match_num = 0;
            Eigen::Matrix3d H = Eigen::Matrix3d::Zero(); // 因为待优化变量维度是3
            Eigen::Vector3d b = Eigen::Vector3d::Zero(); //
            // 1.通过最近邻寻找匹配点
            for (size_t i = 0; i < msg->ranges.size(); i++)
            {
                if (msg->ranges[i] < 0.1 || msg->ranges[i] > 18)
                    continue;
                double real_angle = msg->angle_min + i * msg->angle_increment;
                double x = msg->ranges[i] * cos(real_angle);
                double y = msg->ranges[i] * sin(real_angle);
                Eigen::Vector2d point{x, y};
                Eigen::Vector2d pw = T * point;

                std::vector<int> idx;
                std::vector<float> dis;
                pcl::PointXY p{float(pw.x()),float(pw.y())};
                kdtree_.nearestKSearch(p, 1, idx, dis);
                if(dis[0]>max_dis)
                    continue;
                match_num++;
                //一阶导数
                Eigen::Matrix<double,3,2> J;

                //当前位姿态
                double theta=T.so2().log();
                J<<1,0,0,1,-msg->ranges[i]*std::sin(real_angle+theta),msg->ranges[i]*std::cos(real_angle+theta);
                H+=J.transpose()*J;
                Eigen::Vector2d e(pw.x() - cloud_.points[idx[0]].x,pw.y() - cloud_.points[idx[0]].y );
                b+= -J*e;

                cost+= e.dot(e);

            }

            if(match_num <min_match_num)
                continue;
            Eigen::Vector3d dx = H.ldlt().solve(b);
            cost = cost/match_num;
            if(iter>0&&cost>last_cost)
                 break;
            
            T.translation()+=dx.head<2>();
            T.so2()*= Sophus::SO2d::exp(dx[2]);
            last_cost = cost;
        }
    }

private:
    ros::NodeHandle nh_;
    ros::Subscriber laser_sub_;
    sensor_msgs::LaserScan cur_scan_;
    bool first_scan_ = true;
    pcl::KdTreeFLANN<pcl::PointXY> kdtree_;
    pcl::PointCloud<pcl::PointXY> cloud_;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "icp_p2p_gn");
    ros::NodeHandle nh;
}